#include "modules/map/hdmap/adapter/odr_parser/convert_to_hdmap.h"

#include "modules/map/hdmap/adapter/odr_parser/convert_curve_offset.h"

#include "modules/map/hdmap/adapter/odr_parser/util_odr_parser.h"
#include "modules/map/hdmap/adapter/xml_parser/util_xml_parser.h"

#include "modules/common/configs/config_gflags.h"

#include <cmath>

// Build: M_PI_2 is not defined
#ifndef M_PI_2
#define M_PI_2          1.57079632679489661923
#endif

namespace {
bool IsRoadBelongToJunction(const std::string& road_junctionid) {
  CHECK(!road_junctionid.empty());
  return road_junctionid != "-1";
}
double ToMPS(double speed) { return speed * 1000.0 / 3600.0; }
bool IsReferenceLane(int lane_id) { return lane_id == 0; }

#define tolerance 0.0000001
bool EqualNum(double a, double b) {
  return std::abs(a - b) < tolerance;
}
}  // namespace

namespace apollo {
namespace hdmap {
namespace adapter {

bool OdrToHdmapConverter::reverseXAxis = false;
#define EPS 0.000001

// ra: reverse XAxis
void OdrToHdmapConverter::ConvertRoads(const OdrMap& map_, std::vector<RoadInternal>& roads,
                                       RoadGeometryMap& roadGeometries, bool ra)
{
  reverseXAxis = ra;
  const std::map<std::string, Road*>& odrRoads = map_.roads;
  auto it = odrRoads.begin();
  for (; it != odrRoads.end(); it++) {
    Road* road = it->second;
    RoadInternal road_internal;

    road_internal.id = road->id;
    road_internal.road.mutable_id()->set_id(road->id);

    if (IsRoadBelongToJunction(road->junction_id)) {
      road_internal.road.mutable_junction_id()->set_id(road->junction_id);
    }

    std::int64_t id = std::atoi(road->id.c_str());
    ConvertLaneSections(*road, &road_internal.sections, roadGeometries[id]);
    ConvertObjects(*road, road_internal, roadGeometries[id]);
    ConvertSignals(*road, road_internal);
    
    roads.push_back(road_internal);
  }
}

void OdrToHdmapConverter::ConvertSignals(const Road &road,
                                         RoadInternal& road_internal)
{
    for (size_t i = 0; i < road.signals.size(); i++) {
    const RoadSignal& road_signal = road.signals[i];
    switch (road_signal.type) {
      case ROAD_TRAFFICLIGHT:
        ConvertTrafficLight(road, road_signal, road_internal);
        break;
      default:
        break;
    }
  }
}

void OdrToHdmapConverter::ConvertTrafficLight(const Road &road,
                                              const RoadSignal& road_signal,
                        RoadInternal& road_internal)
{
    std::vector<TrafficLightInternal>& traffic_lights = road_internal.traffic_lights;
    TrafficLightInternal traffic_light_internal;

    traffic_light_internal.id = road_signal.id;
  PbSignal& traffic_light = traffic_light_internal.traffic_light;
    traffic_light.mutable_id()->set_id(road_signal.id);
  for (size_t i = 0; i < road_signal.userData.tags.size(); i++) {
    if (road_signal.userData.tags[i].first == "layoutType") {
      PbSignalType type;
      ToPbLightType(road_signal.userData.tags[i].second, type);
          traffic_light.set_type(type);
    }
    else if (road_signal.userData.tags[i].first == "stoplineId") {
      traffic_light_internal.stop_line_ids.insert(road_signal.userData.tags[i].second);
    }
  }
  for (size_t i = 0; i < road_signal.userData.subSignalIds.size(); i++) {
    PbSubSignal* sub_signal = traffic_light.add_subsignal();
    PbSubSignalType sub_signal_type;
        ToPbSubSignalType(road_signal.userData.subSignalTypes[i], sub_signal_type);
        sub_signal->mutable_id()->set_id(road_signal.userData.subSignalIds[i]);
        sub_signal->set_type(sub_signal_type);
        //PbPoint3D* pt = sub_signal->mutable_location();
  }

  PbPolygon* polygon = traffic_light.mutable_boundary();
  UtilOdrParser::ToLightPolygon(road.planView, road_signal.s, 
                                  polygon,
                  reverseXAxis);

  traffic_lights.push_back(traffic_light_internal);
}

void OdrToHdmapConverter::ToPbLightType(const std::string& layout,
                                        PbSignalType& signal_type)                  
{
  std::string upper_str = UtilXmlParser::ToUpper(layout);

  if (upper_str == "UNKNOWN") {
    signal_type = hdmap::Signal::UNKNOWN;
  } else if (upper_str == "MIX2HORIZONTAL") {
    signal_type = hdmap::Signal::MIX_2_HORIZONTAL;
  } else if (upper_str == "MIX2VERTICAL") {
    signal_type = hdmap::Signal::MIX_2_VERTICAL;
  } else if (upper_str == "MIX3HORIZONTAL") {
    signal_type = hdmap::Signal::MIX_3_HORIZONTAL;
  } else if (upper_str == "MIX3VERTICAL") {
    signal_type = hdmap::Signal::MIX_3_VERTICAL;
  } else if (upper_str == "SINGLE") {
    signal_type = hdmap::Signal::SINGLE;
  } else {
    signal_type = hdmap::Signal::UNKNOWN;
  }
}


void OdrToHdmapConverter::ToPbSubSignalType(const std::string& xml_type,
                                           PbSubSignalType& sub_signal_type)
{
  std::string upper_str = UtilXmlParser::ToUpper(xml_type);

  if (upper_str == "UNKNOWN") {
    sub_signal_type = hdmap::Subsignal::UNKNOWN;
  } else if (upper_str == "CIRCLE") {
    sub_signal_type = hdmap::Subsignal::CIRCLE;
  } else if (upper_str == "ARROWLEFT") {
    sub_signal_type = hdmap::Subsignal::ARROW_LEFT;
  } else if (upper_str == "ARROWFORWARD") {
    sub_signal_type = hdmap::Subsignal::ARROW_FORWARD;
  } else if (upper_str == "ARROWRIGHT") {
    sub_signal_type = hdmap::Subsignal::ARROW_RIGHT;
  } else if (upper_str == "ARROWLEFTANDFORWARD") {
    sub_signal_type = hdmap::Subsignal::ARROW_LEFT_AND_FORWARD;
  } else if (upper_str == "ARROWRIGHTANDFORWARD") {
    sub_signal_type = hdmap::Subsignal::ARROW_RIGHT_AND_FORWARD;
  } else if (upper_str == "ARROWUTURN") {
    sub_signal_type = hdmap::Subsignal::ARROW_U_TURN;
  } else {
    sub_signal_type = hdmap::Subsignal::UNKNOWN;
  }
}

void OdrToHdmapConverter::ConvertObjects(const Road &road,
                                         RoadInternal& road_internal,
                                         const rg::RoadGeometry::Ptr roadGeometry)
{
  if (!roadGeometry)
  {
    AERROR << "Road geometry is not valid:" << road.name;
    return;
  }
  for (size_t i = 0; i < road.objects.size(); i++) 
  {
    const RoadObject& road_obj = road.objects[i];
    if (road_obj.outlines.empty()) 
    {
      continue;
    }
    switch (road_obj.type) 
    {
      case ROAD_STOPLINE:
      {
        ConvertStopLine(road, road_obj, road_internal, roadGeometry);
        break;
      }
      case ROAD_CROSSWALK:
      {
        ConvertCrossWalk(road, road_obj, road_internal, roadGeometry);
        break;
      }
      default:
        break;
    }
  }
}                     

void OdrToHdmapConverter::ConvertCrossWalk(const Road &road,
                                         const RoadObject& road_obj,
                                         RoadInternal& road_internal,
                                         const rg::RoadGeometry::Ptr roadGeometry)
{
  const ObjectOutline& outline = road_obj.outlines[0];
  rg::RefPoint p = roadGeometry->mRoadRefLine.computeRefPointAt(road_obj.s);
  if (road_obj.s > roadGeometry->mRoadRefLine.length())
  {
    glm::vec2 offsetP = p.P + glm::vec2(p.T.x, p.T.y) * 
                    float(road_obj.s - roadGeometry->mRoadRefLine.length());
    p.P = offsetP;
  }
  else if (road_obj.s < 0)
  {
    glm::vec2 offsetP = p.P + glm::vec2(p.T.x, p.T.y) * float(road_obj.s);
    p.P = offsetP;
  }

  glm::mat3 rotation = p.get_rotation();
  glm::mat3 localRotation = glm::mat3(glm::eulerAngleY(road_obj.pitch)
                    *glm::eulerAngleZ(road_obj.hdg-M_PI_2)
                    *glm::eulerAngleX(road_obj.roll));

  PbCrosswalk crosswalk;
  crosswalk.mutable_id()->set_id(road_obj.id);
  PbPolygon* polygon = crosswalk.mutable_polygon();
  for(auto cornerlocal : outline.cornerLocals)
  {
    glm::dvec3 point = glm::dvec3(
      cornerlocal.u, cornerlocal.v, cornerlocal.z);
    point = localRotation * point;
    point.y += road_obj.t;
    point = rotation * point;
    point = point + glm::dvec3(p.P.x, p.P.y, p.E);
    point = point + roadGeometry->mLocalOrigin;
    PbPoint3D* pt = polygon->add_point();
    pt->set_x(point.x);
    pt->set_y(point.y);
    pt->set_z(point.z);
  }
  road_internal.crosswalks.emplace_back(crosswalk);
}  

void OdrToHdmapConverter::ConvertStopLine(const Road &road,
                                         const RoadObject& road_obj,
                                         RoadInternal& road_internal,
                                         const rg::RoadGeometry::Ptr /*roadGeometry*/)
{
  if (road_obj.outlines.empty()) {
    return;
  }
  const ObjectOutline& outline = road_obj.outlines[0];
  if (outline.cornerLocals.size() != 2) {
    return;
  }

  StopLineInternal stop_line;
    stop_line.id = road_obj.id;
    PbCurveSegment* curve_segment = stop_line.curve.add_segment();
    CHECK(curve_segment != nullptr);
  UtilOdrParser::ToStopLineCurve(road.planView, road_obj.s, 
                                  outline.cornerLocals[0].u,
                  outline.cornerLocals[1].u,
                  curve_segment,
                  reverseXAxis);
    road_internal.stop_lines.emplace_back(stop_line);
}  

void OdrToHdmapConverter::ConvertLaneSections(const Road &road,
                std::vector<RoadSectionInternal>* sections,
                const rg::RoadGeometry::Ptr roadGeometry)
{
#ifdef CONVERT_DEBUG
  if (!roadGeometry) {
    printf("ERROR: no road geometry for road %s!!!\n", road.id.c_str());
    return;
  }
  printf("road==========================\n");
  printf("road id: %s\n", road.id.c_str());
  if (road.id == "2398") {
    printf("road 3.\n");
  }
#endif

  size_t section_cnt = 0;
  const std::vector<SectionLaneDivLines>& divLines = roadGeometry->mCachedLaneSectionDivLines;
  glm::dvec3 origin = roadGeometry->mLocalOrigin;
    
  for (size_t i=0; i<road.laneSections.size(); i++) {
    LaneSection* lane_section = road.laneSections[i];
    if (lane_section->left.empty() && lane_section->right.empty()){
      // this lane section only has center lane, skip it
      continue;
    }

    RoadSectionInternal section_internal;

    std::string section_id = std::to_string(++section_cnt);
    section_internal.id = section_id;
    section_internal.section.mutable_id()->set_id(section_id);

    ADEBUG << "lane section: " << (int)i;
    
    ConvertLaneSection(road, *lane_section, road.planView, &section_internal.lanes, divLines[i], origin,
                        road.laneSections);
    ConvertSectionBoundary(*lane_section, section_internal);

    sections->push_back(section_internal);
  }
}

void OdrToHdmapConverter::ConvertLaneSection(
  const Road &road, const LaneSection& lane_section, const PlanView& planview,
  std::vector<LaneInternal>* lanes,
  const SectionLaneDivLines& divLine,
  const glm::dvec3& origin,
  const std::vector<LaneSection*>& laneSections)
{
  Lane* lane;
  Lane* center_lane;

  const double s = lane_section.s;
  const double e = lane_section.e > 0 ? lane_section.e : road.length;
  bool lastSection = (lane_section.e < 0.0);
  double end = road.length - lane_section.s;
  // get reference line from the center line
    
  // for lanes in junction, its boundary is virtual
  bool is_virtual_boundary = IsRoadBelongToJunction(road.junction_id);

  int roadMark_index = -1;
  if (lane_section.center) {
    lane = lane_section.center;
  } else {
    // this laneSection has no center defined, find it
    for (size_t i=0; i<road.laneSections.size(); i++) {
      LaneSection* lane_sec = road.laneSections[i];
      if (lane_sec->left.empty() && lane_sec->right.empty()
            && lane_sec->center){
        // this lane section only has center lane, check it
        if (EqualNum(lane_sec->s, lane_section.s)) {
          lane = lane_sec->center;
        }
        else {
          // find according to road mark
          Lane* tmp_lane = lane_sec->center;
          if (tmp_lane->roadMarks.empty()) {
            lane = tmp_lane;
          }
          else {
            for (size_t i = 0; i < tmp_lane->roadMarks.size() && i < 1; i++) {
              const LaneRoadMark& roadMark = tmp_lane->roadMarks[i];
              if (EqualNum(roadMark.sOffset, lane_sec->s)) {
                roadMark_index = static_cast<int>(i);
                lane = tmp_lane;
                break;
              }
            }
          }
        }
      }
      if (lane) {
        break;
      }
    }
  }

  if (lane) {
    LaneInternal lane_internal;

    ConvertLane(road, *lane, &lane_internal, lastSection, end);
    try {
      //double key = 0.0;
      SetCenterCurve(s, e, &lane_internal, divLine.at(0), origin, is_virtual_boundary);
    } catch (const std::out_of_range& e) {
      printf("Error: not find center lane.\n");
    }

    // fix offset (s) for curve segment
    ConvertCurveOffset::FixCurveOffset(lane_internal);
    SetBorderType(*lane, lane, &lane_internal, roadMark_index);

    //printf("\tlane id: %s, uid: %s\n",lane->id.c_str(), lane->uid.c_str());
    lanes->push_back(lane_internal);
  }

  center_lane = lane; // the center lane can be NULL?

  //PbLaneBoundary* pre_boundary = center_left_boundary;
  const rg::LaneDivRefLine* preRefLine = &divLine.at(0);
  Lane* another_side_lane = center_lane; // initial to center

  for (size_t i = 0; i != lane_section.left.size(); i++) {
    size_t index = lane_section.left.size() - i - 1;
    lane = lane_section.left[index];
    LaneInternal lane_internal;
    ConvertLane(road, *lane, &lane_internal, lastSection, end);

    ADEBUG << "\tleft lane id: " << lane->id <<", uid:" << lane->uid;
    // get matched LaneDivRefLine
    //int id = std::atoi(lane->id.c_str());    

    try{
      SetCenterAndBorderCurve(s, e, &lane_internal,
                                       divLine.at(std::atoi(lane->id.c_str())),
                                       origin,
                                       is_virtual_boundary,
                                       preRefLine, true/*left lane*/);
      preRefLine = &divLine.at(std::atoi(lane->id.c_str()));
    } catch (const std::out_of_range& e) {
      AERROR << "Error: not find center lane.";
    }

    // TODO: set left boundary type for left lanes
    //
    if (index == 0) {
      SetBorderType(*lane, another_side_lane, &lane_internal, roadMark_index);
    } else {
      SetBorderType(*lane, another_side_lane, &lane_internal, -1);
    }
    // TODO: set turn type
    SetNeighbor(lane_section.left, *lane, &lane_internal);
    SetNeighbor(lane_section.right, *lane, &lane_internal);
                SetSampleAssociatesForLeftLane(*lane, &lane_internal);
    SetRoadSampleAssociatesForLeftLane(lane_section.left, *lane, &lane_internal);
    SetLaneLink(*lane, &lane_internal, true);

    // fix offset (s) for curve segment
    ConvertCurveOffset::FixCurveOffset(lane_internal);

    // set right boundary
    // FIXME: reverse the curve from center lane???
    //*(lane_internal.lane.mutable_right_boundary()) = *pre_boundary;
    //pre_boundary = lane_internal.lane.mutable_left_boundary();
    //preRefLine = &divLine[std::atoi(lane->id.c_str())];
    
    lanes->push_back(lane_internal);

    another_side_lane = lane;
  }
  
  preRefLine = &divLine.at(0);
  //pre_boundary = center_right_boundary;
  another_side_lane = center_lane; // initial to center
  for (size_t i=0; i<lane_section.right.size(); i++) {
    lane = lane_section.right[i];
    LaneInternal lane_internal;
    ConvertLane(road, *lane, &lane_internal, lastSection, end);

    ADEBUG <<"\tlane id: " << lane->id << ", uid: " << lane->uid;
    try {
      SetCenterAndBorderCurve(s, e, &lane_internal,
                              divLine.at(std::atoi(lane->id.c_str())),
                              origin,
                              is_virtual_boundary,
                              preRefLine, false/*right lane*/);
      preRefLine = &divLine.at(std::atoi(lane->id.c_str()));
    } catch (const std::out_of_range& e) {
      AERROR << "Error: not find center lane.";
    }

    if (i == 0) {
      SetBorderType(*lane, another_side_lane, &lane_internal, roadMark_index);
    } else {
      SetBorderType(*lane, another_side_lane, &lane_internal, -1);
    }
    // TODO: set turn type
    SetNeighbor(lane_section.right, *lane, &lane_internal);
    SetNeighbor(lane_section.left, *lane, &lane_internal);
    SetSampleAssociates(*lane, &lane_internal);
    SetRoadSampleAssociates(lane_section.right, *lane, &lane_internal);
    SetLaneLink(*lane, &lane_internal, false);

    // fix offset (s) for curve segment
    ConvertCurveOffset::FixCurveOffset(lane_internal);

    // set left boundary
    //*(lane_internal.lane.mutable_left_boundary()) = *pre_boundary;
    //pre_boundary = lane_internal.lane.mutable_right_boundary();
    

    lanes->push_back(lane_internal);
    another_side_lane = lane;
  }
}

// set curve for center lane, i.e., reference line
//
// s: start offset of the lane section
// e: end offset of the lane section
void OdrToHdmapConverter::SetCenterCurve(
                        double s, double e,
            LaneInternal* lane_internal,
            const rg::LaneDivRefLine& refLine,
            const glm::dvec3& origin,
            bool is_virtual_boundary)
{
  //int id = std::stoi(lane.id);

  PbCurve* center_curve = lane_internal->lane.mutable_central_curve();
  UtilOdrParser::BoundaryToCurve(center_curve, refLine, origin, s, e);
  lane_internal->lane.set_length(UtilXmlParser::CurveLength(*center_curve));

  PbLaneBoundary* right_boundary = lane_internal->lane.mutable_right_boundary();
  UtilOdrParser::BoundaryToCurve(right_boundary->mutable_curve(), refLine, origin, s, e);  
  right_boundary->set_virtual_(is_virtual_boundary);

  PbLaneBoundary* left_boundary = lane_internal->lane.mutable_left_boundary();
  UtilOdrParser::BoundaryToCurve(left_boundary->mutable_curve(), refLine, origin, s, e);  
  left_boundary->set_virtual_(is_virtual_boundary);
}

// s: start offset of the lane section
// e: end offset of the lane section
void OdrToHdmapConverter::SetCenterAndBorderCurve(
                        double s, double e,
                        LaneInternal* lane_internal,                        
                        const rg::LaneDivRefLine& refLine,
                        const glm::dvec3& origin,
                        bool is_virtual_boundary,
                        const rg::LaneDivRefLine* preRefLine, bool isLeftLane)
{
	// If the lane is left lane, need to reverse the samples
  PbCurve* center_curve = lane_internal->lane.mutable_central_curve();
  UtilOdrParser::CenterToCurve(center_curve, refLine, origin, s, e, isLeftLane/*reverse if left*/);
  lane_internal->lane.set_length(UtilXmlParser::CurveLength(*center_curve));

  PbLaneBoundary* right_boundary = lane_internal->lane.mutable_right_boundary();
  UtilOdrParser::BoundaryToCurve(right_boundary->mutable_curve(), refLine, origin, s, e, isLeftLane/*reverse if left*/);  
  right_boundary->set_virtual_(is_virtual_boundary);

  PbLaneBoundary* left_boundary = lane_internal->lane.mutable_left_boundary();
  UtilOdrParser::BoundaryToCurve(left_boundary->mutable_curve(), *preRefLine, origin, s, e, isLeftLane/*reverse if left*/);  
  left_boundary->set_virtual_(is_virtual_boundary);
/*
  double cl = UtilXmlParser::CurveLength(*center_curve);
  if (e> 0 && cl > e + EPS) {
    printf("length large than end s.\n");
  }
  cl = UtilXmlParser::CurveLength(*right_curve);
  if (e> 0 && cl > e + EPS) {
    printf("right length large than end s.\n");
  }
  cl = UtilXmlParser::CurveLength(*left_curve);
  if (e> 0 && cl > e + EPS) {
    printf("left length large than end s.\n");
  }
  lane_internal->lane.set_length(UtilXmlParser::CurveLength(*center_curve));
  right_boundary->set_length(UtilXmlParser::CurveLength(*right_curve));
  right_boundary->set_virtual_(is_virtual_boundary);
  left_boundary->set_length(UtilXmlParser::CurveLength(*left_curve));
  left_boundary->set_virtual_(is_virtual_boundary);
  */
}

// @param index - for one road, there may be one section for center lane, and multiple
//                sections for other lane. In the center lane, it may define one roadmark
//                for each other section
void OdrToHdmapConverter::SetBorderType(const Lane& lane, const Lane* left_lane, 
            LaneInternal* lane_internal, int index)
{
  for (size_t i = 0; i < lane.roadMarks.size(); i++) {
    if (index == -1 || index == (int)i) {
      const LaneRoadMark& roadMark = lane.roadMarks[i];
      PbLaneBoundary* lane_boundary = lane_internal->lane.mutable_right_boundary();
      SetLaneBoundaryType(lane_boundary, roadMark);
    }
  }
  //by default if now roadmark(normally inside junction), we allow this side out
  if(lane.roadMarks.size()==0)
  {
    PbLaneBoundary* lane_boundary = lane_internal->lane.mutable_right_boundary();
    auto lane_boundary_type = lane_boundary->add_boundary_type();
    lane_boundary_type->add_types(hdmap::LaneBoundaryType::DOTTED_WHITE);
    lane_boundary_type->set_s(0);

  }
  
  if (!left_lane) {
    printf(" left lane is NULL for road mark\n" );
    return;
  }
  for (size_t i = 0; i < left_lane->roadMarks.size(); i++) {
    if (index == -1 || index == (int)i) {
      const LaneRoadMark& roadMark = left_lane->roadMarks[i];
      PbLaneBoundary* lane_boundary = lane_internal->lane.mutable_left_boundary();
      SetLaneBoundaryType(lane_boundary, roadMark);
    }
  }
  
  //by default if now roadmark(normally inside junction), we allow this side out
  if(left_lane->roadMarks.size()==0)
  {

    PbLaneBoundary* lane_boundary = lane_internal->lane.mutable_right_boundary();
    auto lane_boundary_type = lane_boundary->add_boundary_type();
    lane_boundary_type->add_types(hdmap::LaneBoundaryType::DOTTED_WHITE);
    lane_boundary_type->set_s(0);

  }
}

void OdrToHdmapConverter::SetLaneBoundaryType(PbLaneBoundary* lane_boundary,
                                           const LaneRoadMark& roadMark)
{
  auto lane_boundary_type = lane_boundary->add_boundary_type();
  PbLaneBoundaryTypeType boundary_type;
  switch (roadMark.type) {
    case ROADMARK_SOLID:
    case ROADMARK_SOLID_SOLID:
      if (roadMark.color == ROADMARK_YELLOW) {
        boundary_type = hdmap::LaneBoundaryType::SOLID_YELLOW;
      } else if (roadMark.color == ROADMARK_WHITE ||
                 roadMark.color == ROADMARK_STANDARD) {
        boundary_type = hdmap::LaneBoundaryType::SOLID_WHITE;
      }
    break;
    case ROADMARK_BROKEN:
        case ROADMARK_BROKEN_BROKEN:
      if (roadMark.color == ROADMARK_YELLOW) {
        boundary_type = hdmap::LaneBoundaryType::DOTTED_YELLOW;
      } else if (roadMark.color == ROADMARK_WHITE ||
                 roadMark.color == ROADMARK_STANDARD) {
        boundary_type = hdmap::LaneBoundaryType::DOTTED_WHITE;
      }
      break;
    case ROADMARK_NONE:
      boundary_type = hdmap::LaneBoundaryType::UNKNOWN;
      break;
        case ROADMARK_SOLID_BROKEN:
        case ROADMARK_BROKEN_SOLID:
        case ROADMARK_BOTTS_DOTS:
        case ROADMARK_GRASS:
    case ROADMARK_CURB:
    default:
      boundary_type = hdmap::LaneBoundaryType::UNKNOWN;
      break;
  }

    lane_boundary_type->add_types(boundary_type);
    lane_boundary_type->set_s(roadMark.sOffset);
}

void OdrToHdmapConverter::SetLaneLink(const Lane& lane, 
            LaneInternal* lane_internal, bool is_left)
{
  PbID* pb_lane_id;
  for (size_t i = 0; i < lane.predecessors.size(); i++) {
      const TmpLaneConnection& con = lane.predecessors[i];

      //if (is_left) {
      //  pb_lane_id = lane_internal->lane.add_successor_id();
      //} else 
      {
        pb_lane_id = lane_internal->lane.add_predecessor_id();
      }
      pb_lane_id->set_id(con.laneUid);
  }

  for (size_t i = 0; i < lane.successors.size(); i++) {
      const TmpLaneConnection& con = lane.successors[i];

      //if (is_left) {
      //  pb_lane_id = lane_internal->lane.add_predecessor_id();
      //} else 
      {
        pb_lane_id = lane_internal->lane.add_successor_id();
      }
      pb_lane_id->set_id(con.laneUid);
  }
}        

// only for forward direction
//
void OdrToHdmapConverter::SetNeighbor(const std::vector<Lane*>& lanes, const Lane& lane, 
            LaneInternal* lane_internal)
{
  int id = std::stoi(lane.id);
  bool isLeft = id > 0;

  if (isLeft) {
    for (size_t i = 0; i < lanes.size(); i++) {
      if(lanes[i]->type!=LaneType::DRIVING)//fuck me, apollo will use it to do side pass, we don't want adc drive into bushes
        continue;
      int curLaneId = std::stoi(lanes[i]->id);
        if (curLaneId == id + 1) {
        lane_internal->lane.add_right_neighbor_forward_lane_id()->set_id(lanes[i]->uid);
      } else if (curLaneId == id -1) {
        lane_internal->lane.add_left_neighbor_forward_lane_id()->set_id(lanes[i]->uid);
      }
    }
  }
  else {
    for (size_t i = 0; i < lanes.size(); i++) {
      if(lanes[i]->type!=LaneType::DRIVING)//fuck me, apollo will use it to do side pass, we don't want adc drive into bushes
        continue;
      int curLaneId = std::stoi(lanes[i]->id);
        if (curLaneId == id + 1) {
        lane_internal->lane.add_left_neighbor_forward_lane_id()->set_id(lanes[i]->uid);
      } else if (curLaneId == id -1) {
        lane_internal->lane.add_right_neighbor_forward_lane_id()->set_id(lanes[i]->uid);
      }
    }
  }
}

// calculate width of lane
void OdrToHdmapConverter::SetSampleAssociates(const Lane& lane, LaneInternal* lane_internal)
{
//  const std::vector<LaneWidth>& laneWidth = lane.widths;

  double width;
  double laneLength = lane_internal->lane.length();

  for (double i = 0; i < laneLength; i+= FLAGS_map_resolution) {
  width = UtilOdrParser::GetLaneWidth(lane, i);
  auto left_sample = lane_internal->lane.add_left_sample();
    left_sample->set_s(i);
    left_sample->set_width(width / 2.0);

  auto right_sample = lane_internal->lane.add_right_sample();
    right_sample->set_s(i);
    right_sample->set_width(width / 2.0);
  }

}

// calculate distance from boundary line to reference line
void OdrToHdmapConverter::SetRoadSampleAssociates(
                      const std::vector<Lane*>& lanes,
                        const Lane& lane, LaneInternal* lane_internal)
{
//  const std::vector<LaneWidth>& laneWidth = lane.widths;

  double width;
  double rightWidth, leftWidth;
  int laneId = std::stoi(lane.id);

  double laneLength = lane_internal->lane.length();
  for (double i = 0.0; i < laneLength; i += FLAGS_map_resolution) {
    width = UtilOdrParser::GetLaneWidth(lane, i);
    rightWidth = leftWidth = width / 2.0;

    for (size_t j = 0; j < lanes.size(); j++) {
      Lane* curLane = lanes[j];
      if (curLane->type != DRIVING)
        continue;

      int curId = std::stoi(curLane->id);
        if (curId < laneId) {
        rightWidth += UtilOdrParser::GetLaneWidth(*curLane, i);
      } else if (curId > laneId) {
        leftWidth += UtilOdrParser::GetLaneWidth(*curLane, i);
      }

  }

  auto left_sample = lane_internal->lane.add_left_road_sample();
    left_sample->set_s(i);
    left_sample->set_width(leftWidth);

  auto right_sample = lane_internal->lane.add_right_road_sample();
    right_sample->set_s(i);
    right_sample->set_width(rightWidth);
      
  }
}


// calculate width of lane
void OdrToHdmapConverter::SetSampleAssociatesForLeftLane(const Lane& lane, LaneInternal* lane_internal)
{
  double width;
  double laneLength = lane_internal->lane.length();

  double resolution = UtilOdrParser::CurveResolution;

  for (double i = laneLength; i >= 0; i -= resolution) {
    width = UtilOdrParser::GetLaneWidth(lane, i);
    auto left_sample = lane_internal->lane.add_left_sample();
    left_sample->set_s(laneLength - i);
    left_sample->set_width(width / 2.0);

    auto right_sample = lane_internal->lane.add_right_sample();
    right_sample->set_s(laneLength - i);
    right_sample->set_width(width / 2.0);
  }

}

// calculate distance from boundary line to reference line
void OdrToHdmapConverter::SetRoadSampleAssociatesForLeftLane(
                      const std::vector<Lane*>& lanes,
                        const Lane& lane, LaneInternal* lane_internal)
{
//  const std::vector<LaneWidth>& laneWidth = lane.widths;

  double width;
  double rightWidth, leftWidth;
  int laneId = std::stoi(lane.id);
  double resolution = UtilOdrParser::CurveResolution;

  double laneLength = lane_internal->lane.length();
  for (double i = laneLength; i >=0; i -= resolution) {
    width = UtilOdrParser::GetLaneWidth(lane, i);
    rightWidth = leftWidth = width / 2.0;

    for (size_t j = 0; j < lanes.size(); j++) {
      Lane* curLane = lanes[j];
      if (curLane->type != DRIVING)
        continue;

      int curId = std::stoi(curLane->id);
      if (curId < laneId) {
        rightWidth += UtilOdrParser::GetLaneWidth(*curLane, i);
      } else if (curId > laneId) {
        leftWidth += UtilOdrParser::GetLaneWidth(*curLane, i);
      }
    }

  auto left_sample = lane_internal->lane.add_left_road_sample();
    left_sample->set_s(laneLength - i);
    left_sample->set_width(leftWidth);

  auto right_sample = lane_internal->lane.add_right_road_sample();
    right_sample->set_s(laneLength - i);
    right_sample->set_width(rightWidth);
      
  }
}

void OdrToHdmapConverter::ConvertLane(const Road &road, const Lane& odrLane, LaneInternal* lane_internal, bool lastSection, double end)
{
  PbLane* lane = &lane_internal->lane;
  lane->mutable_id()->set_id(odrLane.uid);
  
  PbLaneType pb_lane_type;
  switch (odrLane.type) {
    case NONE:
      pb_lane_type = hdmap::Lane::NONE;
      break;
    case DRIVING:
      pb_lane_type = hdmap::Lane::CITY_DRIVING;
      break;
    case SIDEwALK:
      pb_lane_type = hdmap::Lane::SIDEWALK;
      break;
    default:
      pb_lane_type = hdmap::Lane::NONE;
      break;
  }
  lane->set_type(pb_lane_type);

  lane->set_speed_limit(ToMPS(odrLane.speedMax));
  
  if(lastSection)
  {
    for (auto& signals: road.signals) {
      OverlapWithLane overlap_with_lane;
      overlap_with_lane.object_id = signals.id;
      overlap_with_lane.start_s = end - 1;
      overlap_with_lane.end_s = end - 1;
      overlap_with_lane.is_merge = false;
      lane_internal->overlap_signals.push_back(overlap_with_lane);
    }
  }
  //lane_boundary->set_virtual_(false);
/*
    for (size_t i = 0; i < odrLane.predecessors.size(); i++) {
      PbID* pb_lane_id = lane->add_predecessor_id();
      pb_lane_id->set_id(odrLane.predecessors[i].laneUid);
  }
    for (size_t i = 0; i < odrLane.successors.size(); i++) {
      PbID* pb_lane_id = lane->add_successor_id();
      pb_lane_id->set_id(odrLane.successors[i].laneUid);
  }
  */
}

// TODO:
void OdrToHdmapConverter::ConvertSectionBoundary(
    const LaneSection& lane_section, RoadSectionInternal& section_internal)
{
  PbBoundaryPolygon* boundary = section_internal.section.mutable_boundary()->mutable_outer_polygon();

  std::string right_lane_id;
  std::string left_lane_id;
  if (!lane_section.right.empty()) {
    right_lane_id = lane_section.right[lane_section.right.size() - 1]->uid;
    left_lane_id = lane_section.right[0]->uid;
    for (size_t i = 0; i < section_internal.lanes.size(); i++) {
      if (section_internal.lanes[i].lane.id().id() == right_lane_id) {
        // 0 for right
        FillBoundary(section_internal.lanes[i], boundary, 0);
      }
      if (section_internal.lanes[i].lane.id().id() == left_lane_id) {
        // 1 for left
        FillBoundary(section_internal.lanes[i], boundary, 1);
      }
    }
    
  }
  
  // TODO: set boundary for left lanes
  //
}

void OdrToHdmapConverter::FillBoundary(LaneInternal& lane_internal,
                                        PbBoundaryPolygon* boundary,
                                        bool is_left)
{
  const PbLaneBoundary& lane_boundary = is_left ? 
                            lane_internal.lane.left_boundary() :
                            lane_internal.lane.right_boundary();

  PbBoundaryEdge* boundary_edge = boundary->add_edge();
  PbCurve* boundary_curve = boundary_edge->mutable_curve();

  const PbCurve& curve = lane_boundary.curve();
  int seg_num = curve.segment_size();
  for (int i = 0; i < seg_num; i++) {
    const PbCurveSegment& curve_segment = curve.segment(i);
    PbCurveSegment* boundary_segment = boundary_curve->add_segment();
    boundary_segment->set_s(curve_segment.s());
    boundary_segment->mutable_start_position()->set_x(curve_segment.start_position().x());
    boundary_segment->mutable_start_position()->set_y(curve_segment.start_position().y());
    boundary_segment->set_length(curve_segment.length());

    const PbLineSegment& line_segment = curve_segment.line_segment();
    PbLineSegment* boundary_line_segment = boundary_segment->mutable_line_segment();

    int sample_rate = 3;
    for (int j = 0; j < line_segment.point_size(); j++) {
      // road boundary line can have less point than lane boundary
      if (j % sample_rate == 0 || j == line_segment.point_size() - 1) {
        PbPoint3D* pt = boundary_line_segment->add_point();
        pt->set_x(line_segment.point(j).x());
        pt->set_y(line_segment.point(j).y());
      }
    }
  }
  
  boundary_edge->set_type(is_left ? 
                          hdmap::BoundaryEdge::LEFT_BOUNDARY :
                          hdmap::BoundaryEdge::RIGHT_BOUNDARY);
}                                        

//#define DEBUG_JUNCTION_OUTLINE

void OdrToHdmapConverter::ConvertJunctions(const OdrMap& map_, 
                                  const std::vector<RoadInternal>& roads,
                                  std::vector<JunctionInternal>& junctions)
{
#ifdef DEBUG_JUNCTION_OUTLINE
  printf("==============Convert Junction==================\n\n");
#endif
  // iterate each junction
  // setup outline
  //
  for (auto junction_pair: map_.junctions) {
    const std::string& junction_id = junction_pair.first;
    const Junction* junction = junction_pair.second;

#ifdef DEBUG_JUNCTION_OUTLINE
  printf("junction id: %s\n", junction_id.c_str());
#endif
    // here, ContactPointType is relative to right lane (even the road has no right lane)
    std::map<std::string, ContactPointType> incoming_road_map; // roads in this junction
    //std::set<std::string> junction_road_set;
    for (size_t i = 0; i < junction->connections.size(); i++) {
      const Connection* con = junction->connections[i];
      if (con->contactPoint == START && !con->laneLinkFroms.empty()) {
        if (std::atoi(con->laneLinkFroms[0].c_str()) < 0) {
          incoming_road_map[con->incomingRoad] = END;
        }
        else {
          incoming_road_map[con->incomingRoad] = START;
        }
      } else if (con->contactPoint == END && !con->laneLinkFroms.empty()) {
        if (std::atoi(con->laneLinkFroms[0].c_str()) < 0) {
          incoming_road_map[con->incomingRoad] = START;
        }
        else {
          incoming_road_map[con->incomingRoad] = END;
        }
      }
      //junction_road_set.insert(con->connectingRoad);
    }
#ifdef DEBUG_JUNCTION_OUTLINE
    for (auto it: incoming_road_map) {
      printf("\tincoming road: %s", it.first.c_str());
      if (it.second == START) printf("  START\n");
      else printf("  END\n");
    }
    printf("\n");
#endif

    // direction vector for one lane, used when there is two points left
    apollo::common::PointENU direction_vec;
    direction_vec.set_x(1.0);
    direction_vec.set_y(0.0);
    direction_vec.set_z(0.0);

    // get all boundary points
    // points are CCW
    std::vector<apollo::common::PointENU> boundary_points; // every two points belong to one road
    for (auto& it: incoming_road_map) {
      const std::string& road_id = it.first;

#ifdef DEBUG_JUNCTION_OUTLINE
      printf("\troad id: %s\t", road_id.c_str());
#endif

      for (size_t ri = 0; ri < roads.size(); ri++) {
        const RoadInternal& road_internal = roads[ri];

        if (road_internal.id == road_id) {
          const Road* road = UtilOdrParser::FindRoad(map_.roads, road_id);
          size_t section_num = road->laneSections.size();
          LaneSection* lane_section;
          if (it.second == START) {
                lane_section = road->laneSections[0];
          } else { // END
                lane_section = road->laneSections[section_num - 1];
          }
          // get first point
          //
          Lane* most_right_driving_lane = NULL;
          if (!lane_section->right.empty()) {
            // get start point of most right driving lane
            int lane_id = 0;
            for (size_t li = 0; li < lane_section->right.size(); li++) {
              Lane* lane = lane_section->right[li];
              if (lane->type == DRIVING && std::atoi(lane->id.c_str()) < lane_id) {
                most_right_driving_lane = lane;
                lane_id = std::atoi(lane->id.c_str());
              }
            }
          } 
          if (!most_right_driving_lane) {
            // get start point of center (reference line)
            most_right_driving_lane = lane_section->center;
          }
          const std::string right_lane_id = most_right_driving_lane->uid;
          // get lane
          const LaneInternal* right_lane_internal = UtilOdrParser::GetLaneInRoad(&road_internal, right_lane_id);
          // get start point of this lane
          apollo::common::PointENU first_point;
          if (it.second == START) {
#ifdef DEBUG_JUNCTION_OUTLINE
            printf("start point of right boundary");
#endif
            first_point = UtilOdrParser::GetStartPointOfLane(right_lane_internal, UtilOdrParser::RIGHT_BOUNDARY);
          }
          else {
#ifdef DEBUG_JUNCTION_OUTLINE
            printf("end point of right boundary");
#endif
            first_point = UtilOdrParser::GetEndPointOfLane(right_lane_internal, UtilOdrParser::RIGHT_BOUNDARY);
            apollo::common::PointENU tmp_point = 
                          UtilOdrParser::GetStartPointOfLane(right_lane_internal, UtilOdrParser::RIGHT_BOUNDARY);
            direction_vec.set_x(tmp_point.x() - first_point.x());
            direction_vec.set_y(tmp_point.y() - first_point.y());
          }
          // get second point
          Lane* most_left_driving_lane = NULL;
          if (!lane_section->left.empty()) {
            // get end point of most left lane
            int lane_id = 0;
            for (size_t li = 0; li < lane_section->left.size(); li++) {
              Lane* lane = lane_section->left[li];
              if (lane->type == DRIVING && std::atoi(lane->id.c_str()) > lane_id) {
                most_left_driving_lane = lane;
                lane_id = std::atoi(lane->id.c_str());
              }
            }
          }

          if (!most_left_driving_lane) {
            // get start point of center (reference line)
            most_left_driving_lane = lane_section->center;
          }
          const std::string left_lane_id = most_left_driving_lane->uid;
          // get lane
          const LaneInternal* left_lane_internal = UtilOdrParser::GetLaneInRoad(&road_internal, left_lane_id);

          apollo::common::PointENU second_point;
          int lane_internal_id = std::atoi(most_left_driving_lane->id.c_str());
          if ((it.second == START && lane_internal_id <= 0) ||
              (it.second == END && lane_internal_id > 0)) {
#ifdef DEBUG_JUNCTION_OUTLINE
            printf("start point of left boundary");
#endif
            second_point = UtilOdrParser::GetStartPointOfLane(left_lane_internal, UtilOdrParser::LEFT_BOUNDARY);
          }
          else {
#ifdef DEBUG_JUNCTION_OUTLINE
            printf("end point of left boundary");
#endif
            second_point = UtilOdrParser::GetEndPointOfLane(left_lane_internal, UtilOdrParser::LEFT_BOUNDARY);
          }
          if (it.second == START) {
            boundary_points.push_back(first_point);
            boundary_points.push_back(second_point);
          } else {  // END
            boundary_points.push_back(second_point);
            boundary_points.push_back(first_point);
          }
#ifdef DEBUG_JUNCTION_OUTLINE
          printf("\tfirst point: %f, %f\n", first_point.x(), first_point.y());
          printf("\tsecond point: %f, %f\n", second_point.x(), second_point.y());
#endif

          break;
        }
      }
    }

    std::vector<apollo::common::PointENU> sorted_points;
    SortPointCCW(boundary_points, sorted_points);    

    // merge points in same line
    std::vector<apollo::common::PointENU> merged_points;
    UtilOdrParser::MegerPointsInSameLine(sorted_points, merged_points);

#ifdef DEBUG_JUNCTION_OUTLINE
    printf("points after merge:\n");
    for (size_t mi = 0; mi < merged_points.size(); mi++) {
      apollo::common::PointENU tp;
      tp = merged_points[mi];
      printf("\t%d   : %f  %f\n", (int)mi, tp.x(), tp.y());
    }
#endif    

    // fix for dead road
    //
    if (merged_points.size() == 2) {
        double dx = merged_points[0].x() - merged_points[1].x();  
        double dy = merged_points[0].y() - merged_points[1].y();  

        double distance = sqrt(dx * dx + dy * dy);

        double vec_length = sqrt(direction_vec.x() * direction_vec.x() + direction_vec.y() * direction_vec.y());

        // add another two points by extending by the lane direction
        apollo::common::PointENU tmp_point;
        for (size_t pi = 0; pi < 2; pi++) {
          apollo::common::PointENU tmp_point3;
          if (pi == 0) {
            tmp_point.set_x(merged_points[pi].x() + direction_vec.x() / vec_length * distance);
            tmp_point.set_y(merged_points[pi].y() + direction_vec.y() / vec_length * distance);
          }
          else {
            tmp_point3.set_x(merged_points[pi].x() + direction_vec.x() / vec_length * distance);
            tmp_point3.set_y(merged_points[pi].y() + direction_vec.y() / vec_length * distance);
            merged_points.push_back(tmp_point3);
          }
        }
        merged_points.push_back(tmp_point);

        std::vector<apollo::common::PointENU> sort_again_points;
        SortPointCCW(merged_points, sort_again_points);
        merged_points.swap(sort_again_points);
    }

    // set up apollo junction
    PbJunction pb_junction;
    pb_junction.mutable_id()->set_id(junction_id);
    PbPolygon* polygon = pb_junction.mutable_polygon();

    for (size_t pi = 0; pi < merged_points.size(); pi++) {
      PbPoint3D* pt = polygon->add_point();
      pt->set_x(merged_points[pi].x());
      pt->set_y(merged_points[pi].y());
      pt->set_z(merged_points[pi].z());
    }

    JunctionInternal junction_internal;
    junction_internal.junction = pb_junction;

    junctions.push_back(junction_internal);
  }

  // setup overlap
}

void OdrToHdmapConverter::SortPointCCW(const std::vector<apollo::common::PointENU>& boundary_points, 
                  std::vector<apollo::common::PointENU>& sorted_points)
{
  
    // get a position inside the junction
    //
    size_t point_num = boundary_points.size();
    assert(point_num >= 2);
    const apollo::common::PointENU& p1 = boundary_points[0];
    const apollo::common::PointENU& p2 = boundary_points[point_num - 1];
    apollo::common::PointENU center_p;
    center_p.set_x((p1.x() + p2.x()) / 2.0);
    center_p.set_y((p1.y() + p2.y()) / 2.0);
    center_p.set_z((p1.z() + p2.z()) / 2.0);
    if (point_num >= 6) {
      const apollo::common::PointENU& p3 = boundary_points[2];
      center_p.set_x((center_p.x() + p3.x()) / 2.0);
      center_p.set_y((center_p.y() + p3.y()) / 2.0);
      center_p.set_z((center_p.z() + p3.z()) / 2.0);
    }

#ifdef DEBUG_JUNCTION_OUTLINE
    printf("\tpoint num: %d\n", (int)point_num);
    printf("\tcenter point: %f, %f\n", center_p.x(), center_p.y());
    printf("\n");
#endif

    // order boundary points ccw
    //
    glm::dvec2 v(1.0, 0.0);
    std::vector<std::pair<double, size_t>> radians_1;
    std::vector<std::pair<double, size_t>> radians_2;
    for (size_t pi = 0; pi < point_num; pi+=2) {
      const apollo::common::PointENU& point = boundary_points[pi];
      glm::dvec2 pv(point.x() - center_p.x(), point.y() - center_p.y());

      // calculate angle with v
      double v_dot;
      double v_length;
      double pv_length;
      double rad;
      v_dot = v[0] * pv[0] + v[1] * pv[1];
      v_length = std::sqrt(v[0] * v[0] + v[1] * v[1]);
      pv_length = std::sqrt(pv[0] * pv[0] + pv[1] * pv[1]);
      rad = std::acos(v_dot / v_length / pv_length);

      if (point.y() > center_p.y()) {
        radians_1.push_back(std::make_pair(rad, pi));
#ifdef DEBUG_JUNCTION_OUTLINE
        printf("\t above y\n");
#endif
      } else {
        radians_2.push_back(std::make_pair(rad, pi));
#ifdef DEBUG_JUNCTION_OUTLINE
        printf("\t below y\n");
#endif
      }

#ifdef DEBUG_JUNCTION_OUTLINE
      printf("\t radian of point %d: %f, vdot: %f\n", (int)pi, rad, v_dot);
#endif
    }

#ifdef DEBUG_JUNCTION_OUTLINE
    printf("\t after sort:\n");
#endif
    // sort according to radians
    std::sort(radians_1.begin(), radians_1.end(), 
                   [](const std::pair<double, size_t>& a,
                      const std::pair<double, size_t>& b) {
           return a.first < b.first;
    });
    std::sort(radians_2.begin(), radians_2.end(), 
                   [](const std::pair<double, size_t>& a,
                      const std::pair<double, size_t>& b) {
           return a.first > b.first;
    });

  
    for (size_t ri = 0; ri < radians_1.size(); ri++) {
      if (ri > 0) {
        // remove duplicated points
        //

      }
      sorted_points.push_back(boundary_points[radians_1[ri].second]);
      sorted_points.push_back(boundary_points[radians_1[ri].second + 1]);
#ifdef DEBUG_JUNCTION_OUTLINE
      apollo::common::PointENU tp;
      tp = boundary_points[radians_1[ri].second];
      printf("\t%d   : %f  %f\n", (int)sorted_points.size()-1, tp.x(), tp.y());
      tp = boundary_points[radians_1[ri].second + 1];
      printf("\t%d   : %f  %f\n", (int)sorted_points.size() -1, tp.x(), tp.y());
#endif
    }
    for (size_t ri = 0; ri < radians_2.size(); ri++) {
      if (ri > 0) {
        // remove duplicated points
        //

      }
      sorted_points.push_back(boundary_points[radians_2[ri].second]);
      sorted_points.push_back(boundary_points[radians_2[ri].second + 1]);
#ifdef DEBUG_JUNCTION_OUTLINE
      apollo::common::PointENU tp;
      tp = boundary_points[radians_2[ri].second];
      printf("\t%d   : %f  %f\n", (int)sorted_points.size()-1, tp.x(), tp.y());
      tp = boundary_points[radians_2[ri].second + 1];
      printf("\t%d   : %f  %f\n", (int)sorted_points.size() -1, tp.x(), tp.y());
#endif
    }
}

}  // namespace adapter
}  // namespace hdmap
}  // namespace apollo
